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ABSTRACT 


A sequential Extended Kalman Filter and Smootning 
routine was deve2loped to provide real time estimates of 
torpedo position and depth on the three dimensional under- 
Baber tracking range at tne Naval Torpedo Station, Keyport, 
Washington. Inputs to tne routine were acoustic pulse 
transit times from the target to receiving array elements 
Migben are non-linear functions of tne position coordinates. 
These inputs were linearized and the filter zgains and 
Meeeoered EStimates calculated on-line. By using a Smoothing 
subroutine, all past filtered estimates were smootned. 
Mises were conducted using simulated toroedo trajectories 
mace traversed multiple nydropnhone arrays. It was found 
tnat filter performance was denendent on system noise and 
M@eedistance tne torpedo was from the hydrophone array and 
the smoothed estimates of states were better than or equal 


Mmemeune tiltered estimates. 
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I. INTRODUCTION 


iaewtoNeoeas KEYDOrt, Wasnington currently operates two 
three-dimensional (3+D) underwater tracking range utilizing 
Meo dar sransmitcer installed in the torpedo to be tracked. 
Maem ctransmicter is synchronized with a master clock. timed 
acoustic pulses are received by bottom mounted hydroonscne 
arrays and tnen relayed via cable to a computer at the 
Boservation site. the computer calculates tne positional 
meordinates of the torpedo and nlots its trajectory through 
the water. 

Tne meaSured data, which consists of the elapsed time 


meen transmission of a pulse until its tee eas 


ay 
() 


ecelot 


hydroonone array, is corrupted witn noise due to the 


© 


combined effects of environmental factors and measurement 
mes urumencts. 

These noisy tracxs are2 later analyzed, and measurements 
mecca MOST inaccurate on the dasis of total track statis- 
mees are removec in order to obtain a smooth representation 
ere uhe track. 

MommopOOrouUnLeY EXiStS for expanding the capability of 
wae system Dy applying a reali time Aalman rilter and post 
Beem omootning routine which can take as an input the 


Meanscit times of the acoustic pulses, and produce the best 





Bovimate Gf che position of the tracked object at a 
Peiseaculear time. Previous research in this area [3] and 
Bee revealed that a Kalman filter utilizing a sequential 
meelmacion approach was desirable. 

Weemigteteoreon is co develop and test a sequential Kalman 
Mmipever and smootning algorithm that can be interfaced with 


the current underwater range system. 





Pee Soon me TONS Or RANGE TRACKING GEOMETRY 


iMNemiyeroodone array, Consisting of four independent 
elements, defines an orthogonal coordinate system in which 
transit time measurements are made. As shown in Figure 1, 
meer hydrophones X, Y, Z, and C are on four adjacent 
vertices separated by a distance d, along the edge of tne 
Cube. tne origin of the array coordinates is at the center 
Memecne cube With the ortnogonal coordinates parallel to its 
Eaee. Positional information is computed from the transit 
memes Of a periodic svnehronous acoustic signal travelling 
Mmeom one torpedo to the four hnydrophones on the array. The 
range measures tne tracked torpedo's position every 1.31 
Meeoncs tO an accuracy that is typically within 3 to 30 
mer. A more detailed description of the range tracking 


Sepaoility is described in [2]. 


c& 
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Figure 1. Geometry of a Tracking Array 





III. THEORY 


A. THE EXTENDED KALMAN FILTER 

Since tne transit times were readily available and are 
melinear functions of position, these equations can be 
Menearized and Kalman filter theory apolied using the 
extended Kalman filter. This procedure produces a real-time 
mrcem, tiit@ring on the transit times lee nae a and qo 
Mepnout the necessity of converting these times to 


Mesitions. 


Pore enes Laree=aimensional location oroblem three 


Bestvion states (x, y, Z) and two velocity states (v,, Vy) 
Meta y target motion. Ine discrete linear and nonlinear 


Oebservation equations are given by 


rare 


x(K + 1) = ? * xCK) + 


° wlk) (25419 
and 
EGKe) eee Gi) yn Ki): OF CK) Gs ) 


in these equations ® and ' are constant matrices anda is a 
Bemeinear function of the state variable x. wk) is plant 
excitation noise and v(k) iS measurement noise. The plant 


noise and measurement noise are assumed uncorrelated (white) 


Beem Zero mean. Tnat is, 





mee we Cd) 1 


Q'(K) 8,5 
and 


E{v(k) « v-(d)] 


iT 


RCK) 6, 


wion 


it 
© 
~ 
Tt. 
cC,e 


Pfjeorder=to apply the linear filter equation (3.2) is 
fawenaded in a Taylor series about the best estimate of the 


State at that time and only tne first-order terms are Keot. 


Memacvion (3.2) gives 


Bowe Ge) * XC K) 4 v(xK) ‘ore 
wnere 
d(x) = 22 (3. 3a) 
~ aC re key <=) 


K(K/K-1) is a predicted value of the state before the kth 
measurement. 
Muesic@ec Crror vector is defined by 


paca / kK) = Bana) ee 
and a predicted state error vector is defined by 
fee 7x-1) 2 X(%/K-1) = x(k) . 


13 





Mew COvsSriance Of State error matrix is defined dy 


ana 


oy 


and 


P(K/k) = ELE(k/k) * 8) Ck/K)1 , 


the predicted covariance of state error matrix is given 


P(k/k=1) = ELR(k/k-1) * % (k/k-1)1 


ive "state exeitatwon Matrix is given Dy 

’ = as aE rn i } 
eGo w=st(x) Elw(k) = w (k)] » f° (x) 
enhewmeasurement noise covariance matrix 15s 


mK) = Sek oy. 


The Kalman filter equations are given by [1]: 


mei) = @Pk/O pe + 200) (3.44) 
G(x) = (k/%=1)8' (kK) LHCk) PC K/e-1) BO (w) + ROODIT!) (3.40) 
Boe) = (1 = GOkK)H(K)] PCk/k-1) (32.4c) 
pees 1 /K) = x'Ck/k) (392) 
Z(k/k-1) = h(x(k/k-1), &) (3.4e) 
eCk) = %(k/k-1) + G(k)([z(k) = 2(k/%-1)] (2a 2) 


14 





Miameo wate ix serves not only to allow for maneuvering 
Memeaisoeto account for any model inaccuracies, that is, any 
discrepancies between the true action of the torpedo and its 
Searacterization by Equation (3.1). The Q also serves to 
prevent the gain matrix G(k) from approaching zero by always 
Mmsuring unecervainty in the predicted covariance of error 


Meter ix P(x+1/k). 


eee Ori IMAL LINEAR SMOOTHING 
Smoothing is a non-real-time data processing scneme tnat 


mfsoeal) measurements between 0 and T to estimate tne state 


ct 


Sea system at certain time t, where 0 < Soe cc 
meecened estimate of x(t) based on all the measurements 
between 0 and T is denoted by X(t/T). 

Smootner error covariance is denoted by P(t/T) and 
meeyi) < P(t) means tnat the smootned estimate of x(t) is 
Mmveys OStter tnan or equal to its filtered estimate. Talis 
Bo snOwn graphically in Figure 2. 


ons may de derived. 


bot 


Several forms of the smootning equat 
Bm@esis the Rauch-Tung-Striebel form, which was used in our 
parvicular case with the discrete-time expressions 


Summarized as follows [71]: 


XRCK/N) = RCK/K) + AL CR(K+1/N) - &(K+1/k) 1 (3.5a) 
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Meetire 2. Advantage of Performing Optima, Smootning (14 


wnere 
ae ae 
AL = PCK/K)8(K) *PCK+1/K) Sn 
PCK/N) = P(CK/K) -- ALCP (K+1/N) 5 PC 


meso for k lcs ps 


tf 


In tnese equations x(k/N) is smoothed 


feeryN) is Error Covariance Matrix Prop 


K+1/K) JA, 


e250 = 1 


am) 
4 


rn 

rd 
oO 

N 


Kx 


SEate EStimave and 





Bie  ROSLEM DEFINITION = TORPEDO TRACKING WITH THE EXTENDED 
CAeve Nee wilen AND OPTIMAL SMOUOLGI: 





fee DL IER EQUATIONS 

In the torpedo tracking problem, the non-linear 
Meeervations are tne four independent transit times from tne 
meracked object to the hydropnones, tee di ty and i Thus 
Bae non-linear measurement matrix z(k) is defined as: 











TCK)| | gael x (ic) +d/2)o Cy (k) 44/2) 24 (2k) 40/299) Pov (ik) 
T Ck) | | gal (x (ic) 4/2) 9+ (yk) 4d /2) 24 (26k) 44/2) 21! P av (x) 
e(k)= = | 
= 
Tk) carl x(k) 4d/2)%+y (x) ad /2) 24 (2k) 44/2) 71 fA ay) 
TC) | | peel (x(k) 44/2) 94 Cy (k) 44/2) 8 (2K) 4/2) 99! Sey (X) 
- ' Ca) 


Boe measurement noises, v(«)'s, are assumed to be zero-mean 
and independent witn a covariance matrix 


I, 


af 9 0 0 
0 3S 0 9 
R(X) = “x (4.2) 
0 0 35 0 
( 2 
0 0 0 g 
ie 


iat 





Demet ene (57524) can be USed CO give the Linearized 
observation matrix. When the derivatives are taken and 
evaluated at the predicted state values x(K/k-1) = x'(k) the 


result is 










x' Ck ey 2 0 y'(k)+d/2 9 zZ'(k)+d/2 
DEN 4 DENY DEN 1 
x'(k)-d/2 9 y'(kK)+d/2 0 Ze 2 
1 DEN2 DEN DEN2 
HK ) Sree (rer 
x weed / 2 0 CK =a / 2 0 2 ead / 2 
DEN 3 DeN 3 DENS 
misc egy 2 0 y'G@k)4+d/2 0 z'(k)-d/2 
DENG DENG DEN4 
where 


D 
DEN1 = [(x"(k)+d/2)@#(y" (x) 4d/2)°4(2' (x) 4d/2)°1 1/2 
: 2 oe 
DEN2 = ee ed) 2 ed 2)°1'" 
2 p Bi 
Osis = aed 2 Crk) 0/2) -4(2' (k)+0/2)°)'** 
A 2 7 2 
DENY = ee a2 0) 2d 2) (z ' (x) -8/2)~1'" 
Mmestorpedo dynamics used for the tracking »mrobdlem are 
assumed to be 1/8° WMihemmeScatawions ‘On five states 4 
Mummers On, x velocity, y position, y veiocity and z position 
(neight of torpedo above hydrophone array). 


Pmlemmeans Of tne random excitation and random noise are 


memeemed to be zero, i.ée., 
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4h 
© 


ELw(k)] 


Ely =) | 


i 
© 


FOur measurements are taken every 1.31 seconds, which is 
. 2 
Mime time slot, and with this sampling time the 1/S” plant 


Meeeestvave transition, (PHI) and gamma, (°) matrices equal 


mo: 
_ leet 
1 T 0 0 0 
0 1 0 0 0 
a = 0 0 1 T 0 (4,4) 
0 0 0 1 0 
0 0 0 0 1 
and 
T°/2 0 9 
T 0 0 
i> | 0 ee 0 CS) 
0 if 0 
0 0 T 


emer macrci x, S8macrix, 8 Matrix, and H matrix are then 


Meeaein the Kalman filter equations (3.4). 





Pee lhe SEQUENTIAL EXTENDED KALMAN FILTER 

In the sequential approacn, the odasic Kalman filter 
mast ons (3.4) must be modified. Calculations are 
performed on each of the four independent transit times in 
mae following order: ae oe ae and ite TOnwedenyi.31 second 
Meee siov. Ine estimate of the states x(«k/k), based on one 
meatisit time measurement are used as tne prediction x(k/K-i1) 
ber the calculations on the next measurement. Thus for the 
first time measurement a Snie, vac rirst row of tne 
Mmetearizing oH Matrix is calculated. 


lem eumemrlrsu ~ain column corresponding to the first 


time measurement ie oer Cul at sap y 


P(k/k-=1) 2 


Gc ee eee. 
iMelegl v } ] ie 
eo eas . Rs 


Mmere 3 = 1 to 4 corresponding to the four measured transit 
Memes, inus, the first row of the H matrix is used to 
Bereulate cwne first column of the gain matrix with botn 


on 
L 


Berresponding to the first measured time 


@) 


Meee, etl sseimate of the oarticular observation time is 
Meculated using equation (3.47) evaluated at the predicted 
Seve x(k/K-1). 


Wresaditterence between observed transit time and tne 


£2, 


[mmmena7ea transit times forms tne residual wnicn is used in 


Ga 
i) 


tne estimate equation 
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k, 5 og eat) ae CG. [Residual | C7) 


eo 


his equation gives an estimate of the states based on one 
Bae cne four time measurements. 
Next, covariance of error is calculated based on one 


measurement by 


wea = Ce. Fi i alee (4.8) 
1 eo “arrow 1-1 
where 
il = 1 diememere matrix 
P } 3 Phew covariance mlaerix calculated from tne 
ecw @usmtisancle time Measuremene or if i = I, 


tnemprediction PCk/x=1). 


meer toe first iteration, xy becomes x(kK/K-1) and P 
Memmemes -(</k=1) tor tne second iteration which calculates 
Mae eStimate of the states sased on the second measurement 


e. 
i 
meeeeG Tour iteracvions (kK = 4), ky becomes tne estimates 
Meme nioeolme slot x(k/kK) and P, becomes the covariance error 
mx/kK). 
ieee oneaicvions fOr tne next time slot are calcuiated 
Meme equations (3.4a) and (3.4d). This process is repeated 


for each time slot. 


2] 





meee Or EMAL SMOOTHING PROCESS 
During the running of tne Extended Kalman filter and 
PmoOotnhing routine, after the forward filter pass for each 


memoeslot (Sxceot tne first), the smootning subroutine is 


OQ 


called. By using the oresent and previous filtered estimate 


SaLculaced . 


$7 


Oeeex(t), a smootned estimate of previous x(t) i 
Mts orocess is repeated for each past time slot. 

Pemue Toy eor pee equaesilons (3.5) sroceeds as follows: As 
mmexamole, and because 1t is slightly easier to seé when 
meoual times are used, suppose N = 30. On the forward 
Mmelcer pass, tne values ReK/ Lo) es ae PCG er and 
P(K/k-1) would be computed and stored. On the final 


Meewecion of the forward pass, with &m = N = 30, 
pave 30) = 4030729) + €(20) (2030) - 4 x(30/29)1 


i.e@., we have computed and stored x(30/30). 


Now, the smoothing vsrocess starts in the reverse 





Memeeccion. Decrement kK to x =N - 1 = 29, ‘then 
pega 20) = x6 29729) + 829) (030/39) - £(30/29)] 
stored SO ed stored 
and = A(29) = P(29/29) #? P(30/29)7' 
stored stored 


ee 





Mewes = Ni = 2 = 28, then 


biG 38) = 4028/23 28) 1x1 29/30) = %(29/28)] 





stored computed stored 
last 
iteration 
and (28) = P(23/28) 9? p(29/28)7| 
Stored stored 


memoo, for each of the two preceding iterations, 


P(29/30) = P(29/29) + A(29){P (30/30) - 2(30/29)] a (29) 


ee one gang Re ee mS TNS 





stored computed stored stored computed 


( 


NO 


8/30) = P(28/28) + A(28)1P (29/30) = P(29/28)] a! (28) 


ne tata hae gd Pe Sn eta ee ed a ee ee 
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Vel oo LING AND SIMULATION 


fee DESCRIPTION 

Tne sequential Extended Kalman Filter and Smoothing 
Moueine is tested using simulated torpedo tracks. A variety 
Of track scenarios were produced to test the filter and 
Sm@eotvning performance during single and multiple arrays 
meacking. 

Sompuiber generaced tracks were tested in the first 
memes Of Straight running, constant depth and constant 
Velocity torpedoes. A variety of track scenarios were used 
Meensiting through multiple quadrants including: 

oe crossma2 noren of the array. 

Pree fOssima diagonally tnrougn tne array. 

The next series of tests demonstrates the ability of the 
Mmever tO cvrackK tnrougn the areas of multiple arrays 
mreluding: 

Iie Crossing above the arrays. 

femeeerossing @iagonally through tne arrays. 

All runs were made with a variaty of initialization 
meeors in DOSition and velocity. 

Zero mean Gaussian noise is added to corrupt tne 


Mmrervyed transit times for all runs. 


IO 
= 





GATING SCHEME 


[4] 


ee ld 

The operation of the filter may be adversely affected by 
large measurement noiss. One error of a relatively large 
magnitude could invalidate the filtered output for many 
subsequent time slots. Before random measurement no1se and 
Meamagom excitations could be added to tne observed times for 
testing, a form of protection was designed to guard against 
MeeescrOpnic failure. This protection is provided by 
Becablishing limits of acceptability for each of the 
measurements. 

Measurement errors can occur because of many factors 
meeriding an error in the transit time of tne acoustic pulse 
Meemarily due vo the receipt of multipath signals tnat have 
bounced off tne surface, pottom or different density layers. 

A three-sigma gate was designed using tne covariance of 
measurement noise (8) and the covariance of estimation error 
mek /K)). 

Momreaen Calculation of a state estimate Cx(k/K)), tne 
Mmeaesest positional covariance of error was used, ¢ltner x, ¥ 


Sie 2, and converted to time in seconds using the average 


£3 


meee ity Of sound in water for Daboo Bay, 4860 ft/sec. Tne 


Mare unen was Written for each time measurement i= 1 to 4: 









P(kK/k) 
largest ne 


(4360.) a 





DO 
Si 





Mme 2abe expands or decreases depending on tne confidence 


rd 


Mevyel of the position estimate and the transit time. Ii 


e 
a 

1 
— 


77) 


ZDIFF, which is the dirference between tne actual tran 
time received and the predicted transit time to a4 particular 
hydropnone, exceeds the gate, the measurement is considered 
meereceDcvable and the filter gain iS Set to Zero causing the 
Mmecer to ignore the data and take the osorediction of the 


Byeates as the estimate 


x(K/k) 5 x(k/k-1) 


An invalid time measurement zeros only tne gain column for 
mm@ee Dercicular hydropnone causing only tnat hydropnone's 


data to be ignored. 


Meee MULTIPLE ARRAY TRACKING 
Meera ltests were performed on tracks in the area of 


Mgemerray. in order to more closely simulate a typical run 


i 


me coe ran 


(D 


, a senem 


D 
= 
4) 
WY 
(2 


designed to track the torvedo 


Oa 


meousn multiple arrays. 

Borst, = cOOordinate system is defined as snown in 
mere 3. The center of the coordinate system is geographi- 
Serly near the entrance to Dabob Bay in the simulation. 
Array number 6 is the closest array to be coordinate center. 
Memecme Simulation array 1 is at 36,000 feet from coordinate 


Be@ver and array 6 is 6,000 feet. The C nvdrophone is 


Mememed tO be the axis location of each array. Then each X 
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Soordinate System for Multiple Array Tracking 
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HYDRO-~Hydrophone Location Matrix 


Figure 3 
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30 | 





Beeevion for the X hydrophone in eacn array is A + 30, eacn 
Y position for the Y nydrophone is Y, + 30, and each Z 
Bosition for the Z nydrophone is Z He305 These [2 
Beetetons, an AYZ position for each of 4 hydrophones in 6 
arrays, were placed into a 6 x 12 matrix HYDRO and 
Mmea1erenced throughout the routine. 

The geometry centered on each array is taken out of tne 
Problem and tne target position is based on a central 
reference. 


The non-linear time equation becomes 





2D 
T = ven | cs - ae + (y - Yeoe + (zZ - Z 5) 


where x Ye eee Polo oOscte lO Of a particular hydro- 


a? 
phone and array being used. 

The decision parameter used to determine the switching 
mumimarray to array is a straignt handoff. If the predicted 
feeoOsivion iS greater than 3,900 feet from the array in use, 
then an index (18) is incremented and she next row of HYDRO 
MS implemented. This placed into the routine the x, y and z 
DOSitions or the nydrophones in the next array. The handoff 
Memecasily oe utilized in real range operations, as the 
Meeiolt times from adjacent arrays are present at tne 
Meee ver fOr a particular time slot. For simulation, it is 


Mmeeumed that in all the arrays eacn axis pointed in tne same 


Meeeeection. in actual range operations each array is tilted 


23 





Meigen ootn the k and Y axis. Since tne true transit times 
Bree cerivyed in a tilted coordinate frame, the filter's 
Pfemimace Of transit time must also pe calculated in a tilted 
Meorainate frame. tne tilt angle measurements along with 
Bremilevel! rectangular coordinates of the array with respect 
memeeme central rectangular coordinate system can be input 
moeeo the matrix HYDRO to rotate the coordinates of each 


eearophone in the array. 





emo cou © 


fe OeRLES ONE 

Figure 5 shows the true trajectory of the torpedo in the 
merizontal X-Y¥ oslane during a straight run through single 
Meeay. lorpedo velocity is 50 knots in the x-direction. 
Meecia! position errors are set to 25 feet for X and Y. 
Memmooiyy errors are set to zero. Figures 6, 7 and 8 depict 
Mme position errors for both Kalman filter and Smootning. 
Measurement noise is added to all runs. Tne steady state X 
and Y position errors ranged between ~-6 and +9 feet through- 
out the trajectory for Kalman filter and ~2 and +4 feet for 
emooening. fhe position errors are computed by subdtracting 
Meeeiitliter ooSlition estimate, «(«/k), for Kalman filter and 
mex/N), fOr Smoothing, from the computer generated true 
Memreelon fOr eacn time slot. Figures 9, 10, 11, 12, and 13 
Mepict che mean square estimation errors of states. These 
Peeimavion errors are obtained oy taking the appropriate 
Meeeeonal terms of the covariance matrix and smootned 


Mevariance matrix. 


Pee OERLES TWO 
Mieure 14 snows the true trajectory of the torvedo in 
M@emoorizontal X-¥ olane, during a crossing run through 


Meme array. lorpedo velocity is 40 knots in X-direction 





mae 25 Knots in Y-direction. The torpedo deptn i3 
Maintained at 300 feet. Figures 15, 16, and 17 depict tne 
M@eicron errors. Since the initial position errors are set 
to zero, the position errors ranged between -3 and +4 feet. 
Mmeemres 16, 19, 20, 21, and 22 depict the mean square 


Mmeamacion errors of states. 


Meee oa kLlES THREE 

Meemre 23 snows the true trajectory orf tne torpedo in 
Meee iorizontal X-Y plane, during a straight run through 
Mebcuple array. Because of storage problem of the computer, 
Mie runs through the multiple array were made for 190 time 
meets. ilornedo yelocity is 50 xnots in X-direction and tne 
depth is 100 feet. figures 24, 25, and 26 show the position 
errors ranged between -6 and 17 feet. Figures 27, 28, 29, 
meeeeand 31 denict the mean square estimation errors of 
meaces. figure 32 shows tne error ellipsoids superimposed 
Memevery eighteenth observation. Tne error ellipsoids are 
expanded to twenty-five times their true value in order tnat 
tney may be seen. The error ellipsoids provide a geometric 
Meeerpretation of tne benavior of tne estimator. Before tne 
Memo-Off point, at tne ninetyth time siot, the major axis 


wae —ax is 


+) 


Mesacion of the error ellipsoid and magnitude o 
were -10.98 degrees and 43.28 feet, respectively. After the 
Nand-off point major axis rotation became 25.3 degrees, and 


Mes Magnitude became 1.6 feet. When the magnitude of an 





axis of the ellipsoid decreases, the conclusion is that tne 
error in the estimate decreases, because the observation 
from the new array has an error covariance ellipse rotated 
over 40 degrees from the present covariance (~-16.94 degree) 
Of the estimate. The ellipsoids are extremely narrow. When 
meoined the resultant covariance is reduced greatly. 

Mmeeure 4 devicts the result of reduction and rotation of the 
peeetpsolids. Figure 33 shows the error ellipsoids before and 


after the nand-off point. 





Figure 4, Result of Rotation and Reduction of the 
Semor Lt osoids 


oe 





Pee SERIES FOUR 

Peon ensss snows tie true trajectory of the torpedo in 
Mee noOrizontal X-f plane, during a crossing run through 
Melctole array. Torpedo velocity is 50 knots in X-direction 
ead 4O Knots in Y-direction. The torpedo depth is main- 
memeged at 300 feet. Initial position errors are set to 25 
Mere or 4 and & and initial velocity errors are set to 5 
h@ets. Figures 35, 26, and 37 show the position and depth 
Meeors. i since the initial position and velocity errors are 
MeemecD 25 icet and 5 knots, the big position errors were 
taken at the beginning of the run. These values were 
Mmemored trom the figures in order to see clearly to the rest 


mm 
rt 
a 


Se che run. igure 


77) 


38, 39, 40, 41, and 42 show the mean 
mare eStimation errors of states for botn filtered and 


smoothed. 


BS 





Vit wc ONC bUS Tenis 


The sequential Extended Kalman Filter and Smoothing 
Setistactorily provided real time estimates of torpedo 
Meoltion and depth. The average of steady state position 
and deoth errors ranged between 3 and i feet for torpedo 
meackS within the specified radial tracking range after 
meeman filter. YTnese errors nad a range of around 1 foot 
eeecoer Smoothing. 

The filter performance was dependent on system noise and 
tne distance the torpedo was from tne hydronhone array and 
the smoothed estimates of states were petter tnan or equal 
Be the filter estimates. 

Implementation at tne rang? computer facilities can be 
meeomplished oy real time Aalman riltering and post run 
Mmeeniing of the raw time data. future tests should inelude 
evaluating filter performance using trajectories generated 
from actual torpedo runs on tne Dabob test range. These 
bests would verify tne adequacy of the noise model in the 
Meecer and the ability of the software to edit arroneosus 
transit time measurements. 

Meemeovacton and reduccion of the error ellivosoids 
meee., the filter error covariance) was most instructive and 


Meee uC iInSlgnt into tne oerformance of the filter. 
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eRe NDA a 


PACCR Maer oCmers TON AND FEATURES 


ime sequential Extended Kalman Filter and Smootning 
meueine used for torpedo tracking is modularized for ease of 
implementation. The program is general in nature and many 


Mumeenle Darameters of the filter are variable including: 


a. The number of states in tne filter -- N 

Bee ine number of random forcing functions -- M 
ec. Tne number of measurements -- JS 

d. Number of time slots -- JTIME 


Mae Constant matrices PHI, R, COVW, and GAMMA are 
m@eeialized in the beginning of the program using data 
Meevements. the filter is initialized with P(1/0) and 

mmr os) (initial covariance of estimation error and states) 
Seeeee Subroutine INIT. The first estimate is at time 1 and 
Memecinues until ITIME = JTIME + 1. True measurement times 
me) are computed using either subroutines TRAJEC or TRAJC3, 
depending on whether single array or multiple array tracking 
Meimolsemented. zitnher subroutine will compute four 


measurement times (T,, T,, Ts Epo cacti ime slot. ihe 
measurement times are corrupted by zero-mean, white Gaussian 
morse using the I1BM-3033 subroutine GGNML. For each of the 


four time measurements the corresponding row of the 





Minearizing matrix is calculated using either subroutine 
CHROW or CHROW3, depending on whether single array or 
Melciple array tracking is used. The corresponding gain 
Meerix Column Gil is then found. These row and column values 
are utilized in forming the covariance of estimation error. 
Bee tor that particular time measurement. Next tne estimate 
Seene Observation time ZHAT from that particular hydropnone 
memeieormed using the subroutine CZHAT or CZHAT3, depending on 
Whether single or multiple array tracking is implemented. 
Meeeresidual ZDIFF(ZI) = ZIC(I1) = ZHAT, is then calculated. 
Painally the estimate of the states XI based on one time 
measurement is calculated and the process is repeated for 
m@edmext measurement. After four iterations, XI becomes the 
state estimate XKK and PI becomes the updated covariance of 
Soueimation error PKK, and the predictions of the states and 
eemeeriances AAKM] and PAAM1 are formed. Finally, for eacn 
meme Slot (except the first) smoothed state estimates, XKKS, 
m@eecOvVariances, PKKS, are formed using tne subroutine 
SMOOTH. PLOTP is used to generate line printer plots and 


eeviG is used to generate VERSATEC plots. 


fee PROGRAM SUBROUTINES 


A brief description of the subroutines are described 


(D 


pelow: 
1. TRAJEC -- This subroutine develops the torpedo 
mempectcory Which is used as truth data for the fiiter. The 


“SI 
~J 





TOroOuuIhe OULOULS True Transit times, ZI(i), and the x, y, 
Mmoeotttons, 10(1), of tne torpedo for each time slot. THe 
PeiorOUTINe is used during Single array tracking. 

2. TRAJC3 -- This subroutine performs the same function 
meen nhdeC bub is used only during multiple array tracking. 

pounce r =— This subroutine generates the initial state 
meerwer x (0/-1l) and initial covariance matrix P(0/-1). 

He CHROW == This Subroutine computes the appropriate 
mem of tne linearizing H matrix. Each row corresponds to 
ee of the four transit time measurements, a i ae ee 
Meese suDroOuULINe is used during single array tracking. 

5. CHROWS -- This subroutine performs the same 


Mmemertons as CAaROW but is used only during multiple array 


meacKking. 
Pee eat’ =—- "(nis subroutine computes the estimated 
Mmeemei:t times for the filter. Four transit times, ZHAT, are 


Semeeulaved corresponding to each of the four true transit 
memes Zi(l). This subroutine is used during single array 
Cracking. 

fomeocunis =—- Subroucvine performs same functions as 


Meaal nowever it is used only during multiple array 


Beacking. 
ewes —=—- This subroutine dévelops an adaptive Q 
Seameax Waten iS a function of the toroedo velocity. Three 


input variables defined in a data statement at the begin 


= 
t 
rs 
ON 


x tne program can be adjusted: 


78 





aa. SIGACC -~- Maximum expected horizontal 
seceleration of the torpedo. 

Uoeeolevly == Maximum expected cnange in vertical 
meolocity. 

ce. SIGCC -- Maximum expected turn rate of the 
meomoedo in the horizontal plane. 

Tne values listed in the program were used and kept 
memovant during the Simulation tests. If the user desires 
mMOt to use the adaptive Q subroutine, software code is 
provided at the beginning of the program to calculate a 
Semocant Q matrix. 

9. GGNML -- This is an IBM-3033 subroutine contained in 
the IMSL library. The routine generates zero mean wnite 
Seussian noise with an RMS value normalized to 1. The main 


mmeerdan Scales the noise and adds it to the transit time 


Measurements. 
oer oUiP == THis iS an ILBM—-3033 subroutine used to 
Memeeace Ghe line printer plots. Information on tnis 


Subroutine can be obtained from tne IMSL library. 

ieee eh OG == This is an IBM—3033 subroutine used to 
femetatve the VYERSATEC plots. Information on this subroutine 
eameoe ObLained from the IMSL library. 

12. SMOOTH -- This subroutine computes the smoothed 


Seece eStimates and covariances. 


vs 





eee ULLLITY PROGRAMS 


These subroutines were designed to be used for 


Mmeoetitvive Matrix and vector manipulations: 


Ps 


a 


eee IN 


PROD -- multiplying two matrices 

MMULT --+- multiplying a matrix and a vector 
VMULT -- multiplying two vectors 

TRANS -- transposing a matrix 

ADD -~ adding two matrices 

SUB -- subtracting two matrices 

RECTP === inversing a matrix 
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